
#include "Serial.h"
#include "stdio.h"
#include "xil_exception.h"

#include "hardware.h"


//XIntc IntcInstance; //deze moet voor beide Uart hetzelfde zijn, als die in serial.h staat is dat niet het geval

/************************** Variable Definitions *****************************/
//#define INTC_DEVICE_ID		  XPAR_INTC_0_DEVICE_ID
//#define INTC_BASEADDR		  XPAR_INTC_0_BASEADDR

/************************** Function Prototypes ******************************/

/*
 * C function to initialize the hardware UART, interrupt
 */
int initializeUART(	XIntc *IntcInstancePtr,
					XUartNs550 *UartInstancePtr,
					u16 UartDeviceId,
					u16 UartIntrId,
					long baud);

int setupIntrSystemUART(	XIntc *IntcInstancePtr,
							XUartNs550 *UartInstancePtr,
							Xuint16 UartIntrId);

void UartNs550IntrHandler(void *CallBackRef, u32 Event, unsigned int EventData);



/*****************************************************************************/
/*
 *	Functions
 *
 *****************************************************************************/
int initializeUART(	XIntc *IntcInstancePtr,
					XUartNs550 *UartInstancePtr,
					u16 UartDeviceId,
					u16 UartIntrId,
					long baud){
	int Status;
	Xuint16 Options;
	// 1) initialiseer hardware Uart

	/*
	 * Initialize the UART driver so that it's ready to use.
	 */
	Status = XUartNs550_Initialize(UartInstancePtr, UartDeviceId);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	XUartNs550_SetBaud(UartInstancePtr->BaseAddress, UartInstancePtr->InputClockHz, baud);

	//TODO selftest -> nodig?
	/*
	 * Perform a self-test to ensure that the hardware was built correctly.
	 */
	Status = XUartNs550_SelfTest(UartInstancePtr);
	if (Status == XST_SUCCESS) {
		xil_printf("Selftest gelukt!, Status: %d \n", Status);
	}
	else if(Status == XST_UART_TEST_FAIL){
		xil_printf("Selftest mislukt, Status: %d \n", Status);
	}

	xil_printf("Hardware geinitialiseerd \n");

	// 2) initialiseer interrupt UART
	//XUartNs550_InterruptHandler(UartInstancePtr);
	//TODO verwijderen
	xil_printf("*** Precheck interrupt ***\n");
	xil_printf("Xintc baseadress: %d \n", IntcInstancePtr->BaseAddress);
	xil_printf("Xintc isReady: %d \n", IntcInstancePtr->IsReady);
	xil_printf("Xintc isStarted: %d \n", IntcInstancePtr->IsStarted);

	Status = setupIntrSystemUART(IntcInstancePtr, UartInstancePtr, UartIntrId);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	xil_printf("setup INtr oke\n");
	// 3) setHandler
	XUartNs550_SetHandler(UartInstancePtr, UartNs550IntrHandler, UartInstancePtr);
	xil_printf("Sethandler ok \n");

	// 4) setOptions
	Options = XUN_OPTION_DATA_INTR | XUN_OPTION_FIFOS_ENABLE;
	XUartNs550_SetOptions(UartInstancePtr, Options);
	xil_printf("Setoptions ok \n");

	return XST_SUCCESS;

}

int setupIntrSystemUART(	XIntc *IntcInstancePtr,
							XUartNs550 *UartInstancePtr,
							Xuint16 UartIntrId){

	int Status;

	//TODO interrupt system kan meerdere keren geinitialiseerd zijn -> opletten dus
	/*
	 * Initialize the interrupt controller driver so that it is ready
	 * to use.
	 */


	/*
	 * Connect a device driver handler that will be called when an interrupt
	 * for the device occurs, the device driver handler performs the
	 * specific interrupt processing for the device.
	 */
	if (UartIntrId == XPAR_INTC_0_UARTNS550_0_VEC_ID){
		xil_printf("vector id com1\n");
	}
	else if(UartIntrId == XPAR_INTC_0_UARTNS550_1_VEC_ID){
		xil_printf("vector id com2\n");
	}
	else{
		xil_printf("geen vector id\n");
	}

	xil_printf("voor connect\n");

	/*
	Status = XIntc_Connect(IntcInstancePtr, UartIntrId,
			   (XInterruptHandler)XUartNs550_InterruptHandler,
			   (void *)UartInstancePtr);
			   */

	IntcInstancePtr->CfgPtr->HandlerTable[UartIntrId].Handler = (XInterruptHandler)UartNs550IntrHandler;
	IntcInstancePtr->CfgPtr->HandlerTable[UartIntrId].CallBackRef = UartInstancePtr;

	xil_printf("Intr connect\n");

	return XST_SUCCESS;


}

int enableInterrupt(XIntc *IntcInstancePtr, Xuint16 UartIntrId){
	int Status;

	/*
	 * Enable the interrupt for the UartNs550.
	 */
	XIntc_Enable(IntcInstancePtr, UartIntrId);
	xil_printf("enable\n");

	/*
	 * Initialize the exception table.
	 */
	Xil_ExceptionInit();
	xil_printf("exceptions\n");

	/*
	 * Register the interrupt controller handler with the exception table.
	 */
	Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
			 (Xil_ExceptionHandler)XIntc_InterruptHandler,
			 IntcInstancePtr);

	/*
	 * Enable exceptions.
	 */
	Xil_ExceptionEnable();

	//TODO verwijderen
	xil_printf("*** Postcheck interrupt ***\n");
	xil_printf("Xintc baseadress: %d \n", IntcInstancePtr->BaseAddress);
	xil_printf("Xintc isReady: %d \n", IntcInstancePtr->IsReady);
	xil_printf("Xintc isStarted: %d \n", IntcInstancePtr->IsStarted);

	return XST_SUCCESS;
}

void UartNs550IntrHandler(void *CallBackRef, u32 Event, unsigned int EventData)
{
	u8 Errors;
	XUartNs550 *UartNs550Ptr = (XUartNs550 *)CallBackRef;

	if (UartNs550Ptr->BaseAddress == XPAR_UARTNS550_0_BASEADDR){
		xil_printf("Reactie op UART 0 \n");
	}
	if (UartNs550Ptr->BaseAddress == XPAR_UARTNS550_1_BASEADDR){
		xil_printf("Reactie op UART 1 \n");
	}

	/*
	 * All of the data has been sent.
	 */
	if (Event == XUN_EVENT_SENT_DATA) {
		xil_printf("Alle data verzonden, eventdata %d\r\n", EventData);

	}

	/*
	 * All of the data has been received.
	 */
	if (Event == XUN_EVENT_RECV_DATA) {
		XUartNs550_RecvByte(UartNs550Ptr->BaseAddress);
		xil_printf("Data aangekomen, EventData %d \n", EventData);

	}

	/*
	 * Data was received, but not the expected number of bytes, a
	 * timeout just indicates the data stopped for 4 character times.
	 */
	if (Event == XUN_EVENT_RECV_TIMEOUT) {

	}

	/*
	 * Data was received with an error, keep the data but determine
	 * what kind of errors occurred.
	 */
	if (Event == XUN_EVENT_RECV_ERROR) {
		Errors = XUartNs550_GetLastErrors(UartNs550Ptr);
	}
}
/*****************************************************************************/
/*
 *	Serial
 *
 *****************************************************************************/
// Default constructor (UART1)
Serial::Serial(){
	DeviceId = XPAR_UARTNS550_0_DEVICE_ID;
	UartNS550.BaseAddress = XPAR_UARTNS550_0_BASEADDR;
	UartNS550.InputClockHz = XPAR_UARTNS550_0_CLOCK_FREQ_HZ;
	//UartNS550.Handler = (XUartNs550_Handler) XUartNs550_InterruptHandler;
	UartIntrId = XPAR_INTC_0_UARTNS550_0_VEC_ID;
	RecvBuffer.head = 0;
	RecvBuffer.tail = 0;
	UartId = 1;

	IntcPtr = getInterruptController();
}

// Constructor (UART1 of UART2)
Serial::Serial(Xuint16 Id){
	UartId = Id;

	if(UartId == 2){
		DeviceId = XPAR_UARTNS550_1_DEVICE_ID;
		UartNS550.BaseAddress = XPAR_UARTNS550_1_BASEADDR;
		UartNS550.InputClockHz = XPAR_UARTNS550_1_CLOCK_FREQ_HZ;
		//UartNS550.Handler = (XUartNs550_Handler) XUartNs550_InterruptHandler;
		UartIntrId = XPAR_INTC_0_UARTNS550_1_VEC_ID;
		RecvBuffer.head = 0;
		RecvBuffer.tail = 0;

		IntcPtr = getInterruptController();
	}
	else{
		DeviceId = XPAR_UARTNS550_0_DEVICE_ID;
		UartNS550.BaseAddress = XPAR_UARTNS550_0_BASEADDR;
		UartNS550.InputClockHz = XPAR_UARTNS550_0_CLOCK_FREQ_HZ;
		//UartNS550.Handler = (XUartNs550_Handler) XUartNs550_InterruptHandler;
		UartIntrId = XPAR_INTC_0_UARTNS550_0_VEC_ID;
		RecvBuffer.head = 0;
		RecvBuffer.tail = 0;
		UartId = 1;

		IntcPtr = getInterruptController();
	}
}

// Initialize UART + baudrate + Interrupt controller
int Serial::begin(long baud){
	int Status;


	Status = initializeUART(IntcPtr, &UartNS550, DeviceId, UartIntrId, baud);
	if (Status == 0){
		xil_printf("begin success\n");
		return XST_SUCCESS;
	}
	else{
		xil_printf("begin failure\n");
		return XST_FAILURE;
	}


}


// Write a character
void Serial::write(Xuint8 c){
	//XUartNs550_SendByte(UartNS550.BaseAddress, c);
	XUartNs550_Send(&this->UartNS550, &c, 1);
	//XUartNs550_Send(&UartNS550, &c, 1);
	//xil_printf("Karakter proberen te verzenden \n");
}


void Serial::write(void){
	for (Index = 0; Index < TEST_BUFFER_SIZE; Index++) {
		SendBuffer[Index] = Index + 'A';
	}
	XUartNs550_Send(&this->UartNS550, SendBuffer, TEST_BUFFER_SIZE);
}

//TODO testen, werkt enkel met gebruik van interrupten
Xuint16 Serial::available(){
	return (BUFFER_SIZE + RecvBuffer.head - RecvBuffer.tail) % BUFFER_SIZE;
}

//
void Serial::flush(){
	RecvBuffer.head = RecvBuffer.tail;
}

/*
 * Is called when the UART sets an interrupt
 * When the UART receives a byte, the interrupt is responded by storing the char
 * and updating the RecvBuffer
*/
void Serial::store_char(Xuint8 c){
	Xuint32 i = (RecvBuffer.head + 1) % BUFFER_SIZE;

	if(i != RecvBuffer.tail){
		RecvBuffer.buffer[RecvBuffer.head] = c;
		RecvBuffer.head = i;
	}
}

//TODO testen, herschrijven naar gebruikt met interrupten zodat deze niet alles blokkeert.
// Receive a character
Xuint8 Serial::read(){
	// if the head isn't ahead of the tail, we don't have any characters
	if (RecvBuffer.head == RecvBuffer.tail){
		return -1;
	}
	else{
		unsigned char c = RecvBuffer.buffer[RecvBuffer.tail];
	    RecvBuffer.tail = (RecvBuffer.tail + 1) % BUFFER_SIZE;
	    return c;
	}
}

/*
void Serial::write(char *str){
	//XUartNs550_Send(&UartNS550, (Xuint8)(&str), sizeof(str));
	for(Xuint32 i = 0; i < sizeof(str); i++){
		write(str[i]);
	}
	/*while(*str){
		write(*str++);
	}*
}

*/
void Serial::print(const char str[]){
	//write(str);
}

//
void Serial::println(){
	print("\r");
	print("\n");
}

//
void Serial::println(const char str[]){
	print(str);
	println();
}







